﻿/* -- Includes ------------------------------------------------------------ */
#ifndef iox_osy// Si la constante n'a pas été définie` le fichier n'a jamais été inclus
#define iox_osy // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus
#include "iox_osy.h"
#endif

#ifndef string // Si la constante n'a pas été définie` le fichier n'a jamais été inclus
#define string  // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus
#include "string.h"
#endif

#ifndef iox_pid // Si la constante n'a pas été définie` le fichier n'a jamais été inclus
#define iox_pid  // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus
#include "iox_pid.h"
#endif

#ifndef stdio // Si la constante n'a pas été définie` le fichier n'a jamais été inclus
#define stdio  // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus
#include "stdio.h"
#endif

#ifndef CAN // Si la constante n'a pas été définie` le fichier n'a jamais été inclus
#define CAN  // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus
#include "CAN.h"
#endif

#ifndef ASSERV // Si la constante n'a pas été définie` le fichier n'a jamais été inclus
#define ASSERV  // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus
#include "Asserv.h"
#endif

/* -- Types --------------------------------------------------------------- */

/* -- Global Variables ---------------------------------------------------- */

// Test De Paul sur les PWM
uint32 nbPasL;
uint32 nbPasR;




uint8 i;
/* -- Module Global Variables --------------------------------------------- */
sint16 s16_return;
sint8 s8Return;

uint32 canMsgNum;
boolean Tourne;

/* -- Module Global Variables --------------------------------------------- */

sint8 s8Error;


/* -- Parametre d'asservissement ------------------------------------------ */



//#define MSG_RX 0
//#define MSG_TX 10

/* -- Module Global Function Prototypes ----------------------------------- */

//void CanSend(char message[]);
//void CanSendAsserv(uint16 commande, uint16 mesure, uint16 asserv, uint16 counter);
void Init(void);


void Init()
{
	open_iox();
    /*config_wd(WD_TIME_1000_MS);
	set_10Vext(ON);
 	 u16_pctPwmR = u16_pctPwmL = 1;
	 tempoL =tempoR= 0; 
			Tourne=true;   
		//	set_digout_1000(OUT5,0);
   stopR = false;
   stopL = false;
	nbPasL = nbPasR = 0;*/
	i=0;/*
	init_digin_function(DIN27, TriggeredFunctionR);
	init_digin_function(DIN28, TriggeredFunctionL);

   s8Return = set_on_off_threshold(DIN27, 10, 11, 0);
   s8Return = set_on_off_threshold(DIN28, 10, 11, 0);
*/
	//Init CAN
	CanInit();

	AsservInit();
	set_relais(ON);

  // wd_triggern();



}


/* -- Implementation ------------------------------------------------------ */


int main(void)
{

   // init system  
    Init();

 


    set_relais(ON);
  	CanSend("#Program Begin");

	// envois d'un message toutes les 3 secondes
	//init_task_call(10,TaskFunctionTest);
	//init_task_call(15,MoteurFunction);

	CanSend("Begin");


   while (true)
   {
		//wd_triggern();       // reload watchdog
		//if(bToProcess)
		//{
		//	bToProcess = false;
			//TaskFunctionTest();
		delay_ms (10);
		CanReceive();
		//}
   }
   return 0;
}
/*
void MoteurFunction(void)
{
	if (Temps++ >50)
	{
		//	char vitesse[16];
		//sprintf(vitesse,"#gauche %d",u16_pctPwmL);
		Temps=0;
		//CanSend(vitesse);
		TaskFunction();
	}
	
//moteur de gauche si on est pas dÃ©jÃ  arrivÃ© a 1024 pas
	if(!stopL)
	{
		//rampe		
			
		if (nbPasL>4096) //contiu
			u16_pctPwmL--;	//deceleration
		else if(u16_pctPwmL < 150)
			u16_pctPwmL++;

		if (u16_pctPwmL<1)
			stopL=true;
		else
			set_digout_1000(OUT1, u16_pctPwmL);
	}
	
// moteur de droite si on est pas dÃ©jÃ  arrivÃ© a 1024 pas
	if(!stopR)
	{
		//rampe
		if (nbPasR>4096) //contiu
			u16_pctPwmR--;	//deceleration
		 else if(u16_pctPwmR < 150)//&&(nbPasR<2048))
			u16_pctPwmR++;
		

		if (u16_pctPwmR<1)
			stopR=true;
		else
			set_digout_1000(OUT2, u16_pctPwmR);

	}
	if (stopR && stopL)
	{
		Temps=0;
		
		TaskFunction();
	
		stopR=stopL=false;
		u16_pctPwmR = u16_pctPwmL=1;
		tempoL=tempoR=1;
		nbPasR=nbPasL=0;
		if (Tourne)
		{
			set_digout_1000(OUT5, 1);
			set_digout_1000(OUT6, 1);
		}
		else
		{
			set_digout_1000(OUT5, 0);
			set_digout_1000(OUT6, 0);
		}		
		Tourne=!Tourne;
	}
}
*/

